//
//  ARSCNView+HKRuler.m
//  HKOCRuler
//
//  Created by lbxia on 2018/11/23.
//  Copyright © 2018年 lbx. All rights reserved.
//

#import "ARSCNView+HKRuler.h"
#import "Utilities.h"

@implementation HitTestRay

- (instancetype)initWithOrigin:(SCNVector3)orign direction:(SCNVector3)direction
{
    if (self = [super init]) {
        self.origin = orign;
        self.direction = direction;
    }
    return self;
}
@end


@implementation FeatureHitTestResult
@end

@implementation ARSCNView (HKRuler)


- (HitTestRay*)hitTestRayFromScreenPos:(CGPoint)point
{
    ARFrame *frame = self.session.currentFrame;
    if (!frame) {
        return nil;
    }
    
    SCNVector3 cameraPos = [Utilities positionFromTransform:frame.camera.transform];
    
    SCNVector3 positionVec = SCNVector3Make(point.x, point.y, 1);
    
    SCNVector3 screenPosOnFarClippingPlane = [self unprojectPoint:positionVec];
    
    SCNVector3 rayDirection = [Utilities SCNVector3Sub:screenPosOnFarClippingPlane right:cameraPos];
    
    rayDirection = [Utilities SCNVector3Normalized:rayDirection];
    
    HitTestRay *ray = [[HitTestRay alloc]initWithOrigin:cameraPos direction:rayDirection];
    
    return ray;
}

- (SCNVector3OBJ*)hitTestWithInfiniteHorizontalPlane:(CGPoint)point pointOnPlane:(SCNVector3)pointOnPlane
{
    HitTestRay *ray = [self hitTestRayFromScreenPos:point];
    
    if (!ray || ray.direction.y > -0.03) {
        return nil;
    }
    
    
    SCNVector3OBJ *s = [Utilities rayIntersectionWithHorizontalPlane:ray.origin direction:ray.direction planeY:pointOnPlane.y];
    
    return s;
}

- (NSArray<FeatureHitTestResult*>*)hitTestWithFeatures:(CGPoint)point
                             coneOpeningAngleInDegrees:(CGFloat)coneOpeningAngleInDegrees
                                           minDistance:(CGFloat)minDistance
                                           maxDistance:(CGFloat)maxDistance
                                            maxResults:(int)maxResults
{
    NSMutableArray<FeatureHitTestResult*>* results = [NSMutableArray array];
    
    ARPointCloud *features = self.session.currentFrame.rawFeaturePoints;
    if (!features) {
        return results;
    }
    
    HitTestRay*ray = [self hitTestRayFromScreenPos:point];
    if (!ray) {
        return results;
    }
    
    CGFloat maxAngleInDeg = MIN(coneOpeningAngleInDegrees, 360)/2;
    CGFloat maxAngle = ((maxAngleInDeg / 180) * M_PI);
    
    const vector_float3 *points = features.points;
    
    for (int i = 0; i < features.count; i++) {
        
        //可能有问题
        SCNVector3 featurePos = SCNVector3FromFloat3(points[i]);
        
        SCNVector3 originToFeature = [Utilities SCNVector3Sub:featurePos right:ray.origin];

        SCNVector3 crossProduct = [Utilities SCNVector3Cross:originToFeature right:ray.direction];
        
        CGFloat featureDistanceFromResult = [Utilities SCNVector3Length:crossProduct];
        
        CGFloat dotTmp = [Utilities dot:ray.direction vec:originToFeature];
        
        SCNVector3 hitTestResult = [Utilities SCNVector3Add:ray.origin right:[Utilities SCNVector3Product:ray.direction right:dotTmp]];
        
        CGFloat hitTestResultDistance = [Utilities SCNVector3Length:[Utilities SCNVector3Sub:hitTestResult right:ray.origin]];
        
        if (hitTestResultDistance < minDistance || hitTestResultDistance > maxDistance) {
            continue;
        }
        
        SCNVector3 originToFeatureNormalized = [Utilities SCNVector3Normalized:originToFeature];
        
        CGFloat angleBetweenRayAndFeature = acos([Utilities dot:ray.direction vec:originToFeatureNormalized]);
        
        if (angleBetweenRayAndFeature > maxAngle) {
            // Skip this feature - is is outside of the hit test cone.
            continue;
        }
        
        FeatureHitTestResult *fh = [[FeatureHitTestResult alloc]init];
        fh.position = hitTestResult;
        fh.distanceToRayOrigin = hitTestResultDistance;
        fh.featureHit = featurePos;
        fh.featureDistanceToHitResult  = featureDistanceFromResult;
        
        
        [results addObject:fh];
    }
    
    if (results.count > maxResults) {
     
        NSIndexSet *set = [[NSIndexSet alloc]initWithIndexesInRange:NSMakeRange(maxResults, maxResults - results.count)];
        [results removeObjectsAtIndexes:set];
    }
    
    
    return results;
}



- (NSArray<FeatureHitTestResult*>*)hitTestWithFeatures:(CGPoint)point
{
    NSMutableArray<FeatureHitTestResult*>* results = [NSMutableArray array];
    
    HitTestRay*ray = [self hitTestRayFromScreenPos:point];
    if (!ray) {
        return results;
    }
    
    
    FeatureHitTestResult *result = [self hitTestFromOrigin:ray.origin direction:ray.direction];
    
    if (result) {
        [results addObject:result];
    }
    
    
    return  results;
}


- (FeatureHitTestResult*) hitTestFromOrigin:(SCNVector3)origin direction: (SCNVector3)direction
{
    
    ARPointCloud *features = self.session.currentFrame.rawFeaturePoints;
    if (!features) {
        return nil;
    }
    
    NSArray<SCNVector3OBJ*>* points = [self fliterWithFeaturesWithVector_float3:features.points count:features.count];
    
    if (!points || points.count == 0) {
        return nil;
    }
    
    SCNVector3 average = SCNVector3Make(0, 0, 0);
    for (int i = 0; i < points.count; i++) {
        
        SCNVector3 tmp = points[i].s;
        average.x += tmp.x;
        average.y += tmp.y;
        average.z += tmp.z;
    }
    average.x /= points.count;
    average.y /= points.count;
    average.z /= points.count;
    
    SCNVector3 originToFeature = [Utilities SCNVector3Sub:average right:origin];
    
    CGFloat dot = [Utilities dot:direction vec:originToFeature];
    SCNVector3 hitTestResult =[Utilities SCNVector3Add:origin right:[Utilities SCNVector3Product:direction right:dot]];
    
    CGFloat hitTestResultDistance = [Utilities SCNVector3Length:[Utilities SCNVector3Sub:hitTestResult right:origin]];
    
    
    FeatureHitTestResult *result = [[FeatureHitTestResult alloc]init];
    
    result.position = hitTestResult;
    result.distanceToRayOrigin = hitTestResultDistance;
    result.featureHit = average;
    result.featureDistanceToHitResult = hitTestResultDistance;
    
    
    return result;
}


//可能有问题
- (NSArray<SCNVector3OBJ*>*) fliterWithFeaturesWithVector_float3:(const vector_float3*) features
                                                           count:(NSInteger)count
{
    NSMutableArray<SCNVector3OBJ*>* points = [NSMutableArray array];
    
    SCNVector3 average = SCNVector3Make(0, 0, 0);
    
    for (int i = 0; i < count; i++) {
        
        SCNVector3 tmp = SCNVector3Make(features[i].x, features[i].y, features[i].z);
        [points addObject:[SCNVector3OBJ objectWith:tmp]];
        average.x += tmp.x;
        average.y += tmp.y;
        average.z += tmp.z;
    }
    
    if (points.count < 3) {
        
        return points;
    }
    
    //平均值
    average.x /= count;
    
    //方差，可能有问题
    CGFloat sum = 0;
    for (int i = 0; i < points.count; i++) {
        SCNVector3 tmp = points[i].s;
        SCNVector3 s = [Utilities SCNVector3Sub:tmp right:average];
        CGFloat length = [Utilities SCNVector3Length:s];
        sum += (length * 100)*(length*100);
    }
    CGFloat variance = sum / (points.count - 1);
    
    //标准差
    CGFloat standard = sqrtf(variance);
    CGFloat σ = variance / standard;
    
    NSMutableIndexSet *set = [[NSMutableIndexSet alloc]init];
    for (int i = 0; i < points.count; i++) {
        
        SCNVector3 point = points[i].s;
        CGFloat tmp = [Utilities SCNVector3Length: [Utilities SCNVector3Sub:point right:average] ] * 100 ;
        
        if (tmp >= 3 * σ) {
            [set addIndex:i];
        }
    }
    
    [points removeObjectsAtIndexes:set];
    
    return points;
}

- (NSArray<SCNVector3OBJ*>*) fliterWithFeatures:(NSArray<FeatureHitTestResult*>*) features
{
    NSMutableArray<SCNVector3OBJ*>* points = [NSMutableArray array];
    
    SCNVector3 average = SCNVector3Make(0, 0, 0);
    
    for (int i = 0; i < features.count; i++) {
        
        SCNVector3 tmp = features[i].position;
        [points addObject:[SCNVector3OBJ objectWith:tmp]];
        average.x += tmp.x;
        average.y += tmp.y;
        average.z += tmp.z;
    }
    
    if (points.count < 3) {
        
        return points;
    }
    
    //平均值
    average.x /= features.count;
    
    //方差，可能有问题
    CGFloat sum = 0;
    for (int i = 0; i < features.count; i++) {
        SCNVector3 tmp = features[i].position;
        SCNVector3 s = [Utilities SCNVector3Sub:tmp right:average];
        CGFloat length = [Utilities SCNVector3Length:s];
        sum += (length * 100)*(length*100);
    }
    CGFloat variance = sum / (points.count - 1);
    
    //标准差
    CGFloat standard = sqrtf(variance);
    CGFloat σ = variance / standard;
    
    NSMutableIndexSet *set = [[NSMutableIndexSet alloc]init];
    for (int i = 0; i < points.count; i++) {
        
        SCNVector3 point = points[i].s;
        CGFloat tmp = [Utilities SCNVector3Length: [Utilities SCNVector3Sub:point right:average] ] * 100 ;
        
        if (tmp >= 3 * σ) {
            [set addIndex:i];
        }
    }
    
    [points removeObjectsAtIndexes:set];
   
    return points;
}

//- (NSArray<SCNVector3OBJ*>*) fliterWithFeatures:(NSArray<vector_float3*>*) features
//{
//    return nil;
//}



@end




































